/*
 * HallCalibration.cpp
 *
 *  Created on: Jun 27, 2011
 *      Author: fqhuy
 */
#include "HallCalibration.hpp"
#include <cmath>

void Hall_Calibration::calculate_parameters()//int row, int column)
{
	short i, j;
	//  Left camera parameters

	for (i = 1; i <= 2 * ncp; i++) {
		if (i % 2 == 1)
			R[i] = xcl[i / 2 + 1];
		else
			R[i] = ycl[i / 2];
	}

	for (i = 1; i <= 2 * ncp; i++) {
		if (i % 2 == 1) {
			A[i][1] = xc3d[i / 2 + 1];
			A[i][2] = yc3d[i / 2 + 1];
			A[i][3] = zc3d[i / 2 + 1];
			A[i][4] = 1;
			A[i][5] = 0;
			A[i][6] = 0;
			A[i][7] = 0;
			A[i][8] = 0;
			A[i][9] = -(xc3d[i / 2 + 1] * xcl[i / 2 + 1]);
			A[i][10] = -(yc3d[i / 2 + 1] * xcl[i / 2 + 1]);
			A[i][11] = -(zc3d[i / 2 + 1] * xcl[i / 2 + 1]);
		} else {
			A[i][1] = 0;
			A[i][2] = 0;
			A[i][3] = 0;
			A[i][4] = 0;
			A[i][5] = xc3d[i / 2];
			A[i][6] = yc3d[i / 2];
			A[i][7] = zc3d[i / 2];
			A[i][8] = 1;
			A[i][9] = -(xc3d[i / 2] * ycl[i / 2]);
			A[i][10] = -(yc3d[i / 2] * ycl[i / 2]);
			A[i][11] = -(zc3d[i / 2] * ycl[i / 2]);
		}
	}

	least_square(2 * ncp, 11);

}
//---------------------------------------------------------------------------
void Hall_Calibration::least_square(int row, int column) {
	short i, j, k, pivot, imax, jmax, work;
	short colum_order[12], ir[12], jr[12];
	double a[12][23], b[12][12], d, p, q;
	double At[12][MAX_NUM_CALIB_MARKERS * 2], AtR[12], AtA[12][12],
			AtA_1[12][12];

	/* At(A?????)??? */

	for (i = 1; i <= column; i++) {
		for (j = 1; j <= row; j++) {
			At[i][j] = A[j][i];
		}
	}

	//  Tomo->Tomo->Memo1->Lines->Add("At matrix is:");
	//      for(i=1;i<=column;i++){
	//          Tomo->Tomo->Memo1->Lines->Add(FloatToStr(At[i][1])+" "+
	//                                   FloatToStr(At[i][2])+" "+
	//                                   FloatToStr(At[i][3])+" "+
	//                                   FloatToStr(At[i][4])+" "+
	//                                   FloatToStr(At[i][5])+" "+
	//                                   FloatToStr(At[i][6])+" "+
	//                                   FloatToStr(At[i][7])+" "+
	//                                   FloatToStr(At[i][8])+" "+
	//                                   FloatToStr(At[i][9])+" "+
	//                                   FloatToStr(At[i][10])+" "+
	//                                   FloatToStr(At[i][11])+" "+
	//                                   FloatToStr(At[i][12]));
	//      }

	/* AtR(A?????*R)??? */

	for (i = 1; i <= column; i++) {
		AtR[i] = 0;
		for (j = 1; j <= row; j++) {
			AtR[i] += At[i][j] * R[j];
		}
	}

	//  Tomo->Tomo->Memo1->Lines->Add("AtR matrix is:");
	//      for(i=1;i<=column;i++){
	//          Tomo->Tomo->Memo1->Lines->Add(FloatToStr(AtR[i]));
	//      }

	/* AtA(A?????*A)??? */

	for (i = 1; i <= column; i++) {
		for (j = 1; j <= column; j++) {
			AtA[i][j] = 0;
			for (k = 1; k <= row; k++) {
				AtA[i][j] += At[i][k] * A[k][j];
			}
		}
	}

	//  Tomo->Tomo->Memo1->Lines->Add("AtA matrix is:");
	//      for(i=1;i<=column;i++){
	//          Tomo->Tomo->Memo1->Lines->Add(FloatToStr(AtA[i][1])+" "+
	//                                   FloatToStr(AtA[i][2])+" "+
	//                                   FloatToStr(AtA[i][3])+" "+
	//                                   FloatToStr(AtA[i][4])+" "+
	//                                   FloatToStr(AtA[i][5])+" "+
	//                                   FloatToStr(AtA[i][6])+" "+
	//                                   FloatToStr(AtA[i][7])+" "+
	//                                   FloatToStr(AtA[i][8])+" "+
	//                                   FloatToStr(AtA[i][9])+" "+
	//                                   FloatToStr(AtA[i][10])+" "+
	//                                   FloatToStr(AtA[i][11]));
	//      }

	/* AtA-1(A?????*A????)??? */

	for (i = 1; i <= column; i++) {
		for (j = 1; j <= column; j++) {
			a[i][j] = AtA[i][j];
		}
		for (j = column + 1; j <= column * 2; j++) {
			if (i == j - column)
				a[i][j] = 1.0;
			else
				a[i][j] = 0.0;
		}
	}

	// ??????

	for (j = 1; j <= column; j++) {
		colum_order[j] = j;
	}

	for (k = 1; k < column; k++) {
		d=fabs(a[k][k]);
		pivot = 0;
		for (i = k; i <= column; i++) {
			for (j = k; j <= column; j++) {
				if (d < fabs(a[i][j])) {
					d = fabs(a[i][j]);
					imax = i;
					jmax = j;
					pivot = 1;
				}
			}
		}

		if (pivot == 0)
			continue;

		for (j = 1; j <= column * 2; j++) { // ??????
			d = a[k][j];
			a[k][j] = a[imax][j];
			a[imax][j] = d;
		}
		for (i = 1; i <= column; i++) { // ??????
			d = a[i][k];
			a[i][k] = a[i][jmax];
			a[i][jmax] = d;
		}
		work = colum_order[k]; // ??????????
		colum_order[k] = colum_order[jmax];
		colum_order[jmax] = work;
	}

	for (k = 1; k <= column; k++) { // ??????
		p = a[k][k];
		for (j = 1; j <= column * 2; j++) {
			a[k][j] = a[k][j] / p;
		}
		for (i = 1; i <= column; i++) {
			if (i == k)
				continue;
			q = a[i][k];
			for (j = 1; j <= column * 2; j++) {
				a[i][j] -= q * a[k][j];
			}
		}
	}

	// ???????????????????????????????????

	for (i = 1; i <= column; i++) {
		for (k = 1; k <= column; k++) {
			if (colum_order[k] == i) {
				for (j = 1; j <= column; j++) {
					AtA_1[i][j] = a[k][j + column];
				}
			}
		}
	}

	//  Tomo->Tomo->Memo1->Lines->Add("AtA-1 matrix is:");
	//     for(i=1;i<=column;i++){
	//         Tomo->Tomo->Memo1->Lines->Add(FloatToStr(AtA_1[i][1])+" "+
	//                                  FloatToStr(AtA_1[i][2])+" "+
	//                                  FloatToStr(AtA_1[i][3])+" "+
	//                                  FloatToStr(AtA_1[i][4])+" "+
	//                                  FloatToStr(AtA_1[i][5])+" "+
	//                                  FloatToStr(AtA_1[i][6])+" "+
	//                                  FloatToStr(AtA_1[i][7])+" "+
	//                                  FloatToStr(AtA_1[i][8])+" "+
	//                                  FloatToStr(AtA_1[i][9])+" "+
	//                                  FloatToStr(AtA_1[i][10])+" "+
	//                                  FloatToStr(AtA_1[i][11]));
	//     }

	/* AtA-1AtR(A?????*A?????A?????*R???)??? */

	for (i = 1; i <= column; i++) {
		C[i] = 0.0;
		for (j = 1; j <= column; j++) {
			C[i] += AtA_1[i][j] * AtR[j];
		}
	}

	return;
}
